#include "KineticMotion.h"
#include <math.h>


void CKineticMotion::doUpdate(void)
{
	if(!m_initialise)
	{
		y[0]=m_initialPosition.x;
		y[1]=m_initialPosition.y;
		y[2]=m_initialPosition.z;
		y[3]=m_initialVelocity.x;
		y[4]=m_initialVelocity.y;
		y[5]=m_initialVelocity.z;
		for(int i=0; i<6; i++)
		{
			dydt[i]=0;
		}
		m_initialise=true;
	}
	if(m_time==0)
		setValue(m_initialPosition);
	else if(m_time-m_previousTime>=m_step)
	{
		calculDerivative();
		calculPosition();
		setValue(Vector3(y[0], y[1], y[2]));
		m_previousTime=m_time;
	}
}

void CKineticMotion::calculDerivative()
{
	const float GEarthMass=3.99e14;
	const float earthRadius=6.38e6;
	const float r = earthRadius+y[2];
	const float dragFactor=0.3*exp(-0.1238*y[2]);

	dydt[0]=y[3];
	dydt[1]=y[4];
	dydt[2]=y[5];

	
	dydt[3]=-y[3]*dragFactor/m_mass;
	dydt[5]=-y[5]*dragFactor/m_mass;

	float GForce=-(GEarthMass*m_mass)/(r*r);
	float dragForce=-y[4]*dragFactor;
	dydt[4]=(GForce+dragForce)/m_mass;

}

void CKineticMotion::calculPosition()
{
	for(int i=0; i<6; i++)
	{
		y[i]+= m_step*dydt[i];
	}		
}
